/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 * File: mecanum_fk.c
 *
 * MATLAB Coder version            : 5.5
 * C/C++ source code generated on  : 08-Jul-2024 14:50:11
 */

/* Include Files */
#include "mecanum_fk.h"

/* Function Definitions */
/*
 * Arguments    : float w1
 *                float w2
 *                float w3
 *                float w4
 *                float *vx
 *                float *vy
 *                float *wz
 * Return Type  : void
 */
void mecanum_fk(float w1, float w2, float w3, float w4, float *vx, float *vy,
                float *wz)
{
  static const float a[12] = {
      0.25F, -0.25F, -0.568181813F, 0.25F, 0.25F,  0.568181813F,
      0.25F, 0.25F,  -0.568181813F, 0.25F, -0.25F, 0.568181813F};
  float odom[3];
  int i;
  /*      R = 0.04; */
  /*      odom = R/4*[1, 1, 1, 1; -1, 1, 1, -1; -tmp, tmp, -tmp, tmp]*[w1/R;
   * w2/R; w3/R; w4/R]; */
  for (i = 0; i < 3; i++) {
    odom[i] = ((a[i] * w1 + a[i + 3] * w2) + a[i + 6] * w3) + a[i + 9] * w4;
  }
  *vx = odom[0];
  *vy = odom[1];
  *wz = odom[2];
}

/*
 * File trailer for mecanum_fk.c
 *
 * [EOF]
 */
